
#include "stdlib.h"
#include "ringbuff.h"
#include "robot_interact.h"
#include "power_ctrl.h"
#include "serialHMI_analysis.h"
#include "offline_param.h"
#include "user_struct.h"

STR_INTE_PAIR_FRAME pair_frame;
STR_INTE_CTRL_FRAME ctrl_frame;


STR_INTE_HDL robot_hdl;

STR_HMI_FRAME robotAckFrame;

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

static void byte_data_copy(uint8_t *dest, uint8_t *source, uint16_t length)
{
	uint8_t *traget = dest;
	uint8_t *start  = source;
	
	for(uint16_t i = 0; i < length; i++)
	{
		*(traget++) = *(start++);
	}
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void robot_interact_frame_init(void)
{
	robot_hdl.ID_station = AppParamHdl.device_num;
	
	robotAckFrame.head = HMI_FRAME_HEAD;
	robotAckFrame.databuf = &robot_hdl.ack.data[0];
	robotAckFrame.length = HMI_COMM_FRAME_LENGTH;
	robotAckFrame.ID = ID_ROBOT_ACK_FRAME + 0x00EF;

}




/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void reset_robot_station_motor(STR_INTE_HDL *robotHdl)
{
	robotHdl->ctrl.en_actuate_X1 = 0;
	robotHdl->ctrl.en_actuate_X2 = 0;
	robotHdl->ctrl.en_waste_pump = 0;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void reset_robot_info(STR_INTE_HDL *robotHdl)
{
	memset(&robotHdl->pair, 0, sizeof(robotHdl->pair));
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void reset_robot_overtime_cnt(STR_INTE_HDL *robotHdl)
{
	robotHdl->overtime_cnt = 0;
	robotHdl->overtime_flg = 0;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void robot_overtime_check(STR_INTE_HDL *robotHdl, uint16_t runtime_ms)
{
	robotHdl->overtime_cnt += runtime_ms;
	if(robotHdl->overtime_cnt > ROBOT_COMM_OVERTIEM_MS)
	{
		robotHdl->overtime_cnt = ROBOT_COMM_OVERTIEM_MS;
		robotHdl->overtime_flg = 1;	
	}
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void robot_ack_store(STR_INTE_ACK_FRAME *ack_frame)
{
	ack_frame->type = HMI_USER_ACK;	
	ack_frame->ID_station = robot_hdl.ID_station;
	ack_frame->fun_num = robot_hdl.pair.fun_num;
	
	ack_frame->charge_time = chargeCtrl.charge_time_s;
	ack_frame->pair_status = robot_hdl.pair_status;
	
	ack_frame->status_actuate_X1 = 0;
	ack_frame->status_actuate_X2 = 0;
	ack_frame->status_waste_pump = 0;
	ack_frame->waste_level_X1 = 0;
	ack_frame->waste_level_X2 = 0;
}




/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void robot_paried_handle(STR_INTE_HDL *robotHdl, uint16_t runtime_ms)
{
	static uint16_t  pair_check_cnt = 0;
	
	robot_overtime_check(robotHdl, runtime_ms);
	
	if((chargeCtrl.FSM != CHARGE_ON) || (robotHdl->overtime_flg == 1))
	{
		pair_check_cnt = 0;
		robotHdl->pair_status = ENUM_UNPAIRED;
	}	

	switch(robotHdl->pair_status)
	{
		case ENUM_UNPAIRED:
		{	
			reset_robot_info(robotHdl);
			reset_robot_station_motor(robotHdl);

			if(chargeCtrl.FSM == CHARGE_ON)
			{
				robotHdl->pair_status = ENUM_PAIRING;
			}
		
			break;
		}
		
		case ENUM_PAIRING:
		{	
			int16_t charge_time_erro = 0;
			
			charge_time_erro = (int16_t)robotHdl->pair.charge_time - (int16_t)chargeCtrl.charge_time_s;
			
			if((abs(charge_time_erro) < CHARGE_TIME_ERRO_THD_S) && (robotHdl->pair.charge_time != 0))
			{
				pair_check_cnt += runtime_ms;
				if(pair_check_cnt > PAIRED_CHECK_TIME_MS)
				{
					pair_check_cnt = 0;
					robotHdl->pair_status = ENUM_PAIRED;					
				}
			}
			else
			{
				pair_check_cnt = 0;
			}
		
			break;
		}
		
		
		case ENUM_PAIRED:
		{
						
			
			break;
		}
		
		default:
			break;
	}
	
	
	userHdl.ctrl.en_actuate_X1 = robotHdl->ctrl.en_actuate_X1;
	userHdl.ctrl.en_actuate_X2 = robotHdl->ctrl.en_actuate_X2;
	userHdl.ctrl.en_waste_pump = robotHdl->ctrl.en_waste_pump;
}




/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void robot_comm_data_handle(uint8_t *data_temp, STR_RINGBUF *sendbuf)
{
	uint8_t fun_num = data_temp[1];
	
	switch(fun_num)
	{
		case ROBOT_PAIR:
		{
			byte_data_copy(&robot_hdl.pair.data[0], data_temp, 8);
			
			break;
		}
		
		case ROBOT_CTRL:
		{
			if(robot_hdl.pair_status == ENUM_PAIRED)
			{
				byte_data_copy(&robot_hdl.ctrl.data[0], data_temp, 8);
			}
						
			break;
		}
		
		default:
			break;
	}
	
	robot_ack_store(&robot_hdl.ack);
	robotAckFrame.ID = ID_ROBOT_ACK_FRAME + robot_hdl.pair.ID_robot;
	send_buf_packing(sendbuf, &robotAckFrame);
	
	reset_robot_overtime_cnt(&robot_hdl);
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/




